/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.sunspotworld;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.sensorboard.peripheral.Servo;

/**
 *
 * @author Administrator
 */
public class Maze {
    
    // wall constants
    public static final byte UNKNOWN = 0x0; // 00
    public static final byte WALL = 0x1; // 01
    public static final byte OPEN = 0x2; // 10
    public static final byte BEACON = 0x3; // 11
    
    // explored constants
    public static final byte UNEXPLORED = 0x0;
    public static final byte EXPLORED = 0x1;
    public static final byte BEACON_VISIBLE = 0x2;
    
    // each two bits represents a wall starting from the most significant bits they represent
    // top, right, bottom, left
    // each two bits represents:
    //      * 00 - unknown
    //      * 01 - wall
    //      * 10 - no wall 
    //      * 11 - beacon wall
    public byte[][] walls = new byte[25][25];
    
    // bit meaning starting from least significant bit
    // 2^0 = explored flag
    // 2^1 = beacon visible flag
    public byte[][] explored = new byte[25][25];
    // heading in degrees 0, 90, 180, 270
    public int heading = 0;
    // the x,y position in the maze acting as indexes into the walls and explored arrays
    public int x = 12;
    public int y = 12;
   
    public Maze(  ){
        //initialize the walls and the explored arrays to 0 to represend unexplored areas
        for(int column = 0; column < 25; column++){
            for(int row = 0; row < 25; row++ ){
                walls[column][row] = this.UNKNOWN;
                explored[column][row] = this.UNEXPLORED; 
            }
        } 
        
        // mark the initial position as explored
        this.markExplored();
    }
    
    // there was a wall in front of the bot
    // there was a wall to the left of the bot
    public void turnRight(){
       //this.idFrontWall( this.WALL );
       //this.idLeftWall( this.WALL );
       heading = heading + 90;
       heading = heading % 360;
    }
    
    // there is no wall to the left of the bot
    public void turnLeft(){
       //this.idLeftWall( this.OPEN );
       heading = heading + 270;
       heading = heading % 360;
    }
    
    // there is no wall in front of the bot
    // there is a wall to the left of the bot
    // only change the left wall if it is unknown because in the case of a turn
    // left then go forward we do not want to mark the left as a wall
    public void goForward( ){   
        //this.idFrontWall( this.OPEN );
        //this.idLeftWall( this.WALL );
        
        // heading up add subtract one from columns
        if( heading == 0 ){
          //  y--;
        }
        else if( heading == 90 ){
           // x++;
        }
        else if( heading == 180 ){
           // y++;
        }
        else if( heading == 270 ){
           // x--;
        }
        
        //this.markExplored();
    }
    
    // There is a beacon wall directly in front of the bot.
    // Probably good to call this when the bot hits the beacon.
    // We may not want to use this at all and just let the bot find the beacon
    // from one of the beacon visible squares
    public void markBeaconWall(){
        //this.idFrontWall( this.BEACON );
    }
    
    // mark the position of the beacon.  This does not mean the beacon is in
    // this square it just means that the beacon is visible
    public void markBeaconVisible(){
        //explored[x][y] = (byte)(explored[x][y] | this.BEACON_VISIBLE);
    }
    
    // mark the current position on the map as explored
    private void markExplored(){
        //explored[x][y] = (byte)(explored[x][y] | this.EXPLORED);
    }
    
    private void idFrontWall( byte type ){
        byte frontWall = 0;
        byte mask = 0x3;
        if( heading == 0 ){
            frontWall = (byte)(type << 6);
            mask = (byte)(mask << 6);
        } else if( heading == 90 ){
            frontWall = (byte)(type << 4);
            mask = (byte)(mask << 4);
        } else if( heading == 180 ){
            frontWall = (byte)(type << 2);
            mask = (byte)(mask << 2);
        } else if( heading == 270 ){
            frontWall = (byte)type;
            mask = (byte)mask;
        }
    }
    
    private void idLeftWall( byte type ){
        byte leftWall = 0;
        if( heading == 0 ){
            leftWall = (byte)type;
        } else if( heading == 90 ){
            leftWall = (byte)(type << 6);
        } else if( heading == 180 ){
            leftWall = (byte)(type << 4);
        } else if( heading == 270 ){
            leftWall = (byte)(type << 2);
        }
    }
    
    private void idRightWall( byte type ){
        byte rightWall = 0;
        if( heading == 0 ){
            rightWall = (byte)(type << 4);
        } else if( heading == 90 ){
            rightWall = (byte)(type << 2);
        } else if( heading == 180 ){
            rightWall = (byte)type;
        } else if( heading == 270 ){
            rightWall = (byte)(type << 6);
        }
    }
    
    public void print(){
        String map;
        for(int row = 0; row < 25; row++){
            System.out.print(" ");
            for(int column = 0; column < 25; column++ ){
                // print the top wall
                System.out.print(getChar(walls[column][row] >> 6));
                System.out.print(" ");
            }
            System.out.print("\n");
            for(int column = 0; column < 25; column++ ){
                // print the side walls
                System.out.print(getSideChar(walls[column][row]));
                System.out.print(" ");
                System.out.print(getSideChar(walls[column][row] >> 4));
            }
            System.out.print("\n");
            System.out.print(" ");
            for(int column = 0; column < 25; column++ ){
                // print the bottom wall
                System.out.print(getChar(walls[column][row] >> 2));
                System.out.print(" ");
            }
            System.out.print("\n");
        } 
    }
    
    private char getChar( int bits ){
        if( (bits & this.WALL) == this.WALL )
            return (char)196;
        if( (bits & this.UNKNOWN) == this.UNKNOWN )
            return (char)248;
        if( (bits & this.BEACON) == this.BEACON )
            return (char)237;
        
        return (char)32;
        
    }
    
    private char getSideChar( int bits ){
        if( (bits & this.WALL) == this.WALL )
            return (char)179;
        if( (bits & this.UNKNOWN) == this.UNKNOWN )
            return (char)248;
        if( (bits & this.BEACON) == this.BEACON )
            return (char)237;
        
        return (char)32;
    }
    
    public void findPath(){
        
    }
    
    public String getNextCommand(){
        return( "FORWARD" );
    }
    
    
    
    
    
  
}
